
close all
clear 
pkg load symbolic

syms rx ry Vc_x Vc_y Vc_w v1 v2 v3 v4 r theta1 theta2 theta3 theta4

Vc_x =0.5;%m/s
Vc_y =0;
Vc_w =0.262;%rad/s

ry=0.200;%m
rx=0.260+0.0;%m

K=[[1 0 -ry];
   [0 1 rx];
   [1 0 -ry];
   [0 1 -rx];
   [1 0 ry];
   [0 1 -rx];
   [1 0 ry];
   [0 1 rx]]

   M=pinv(K);
   
   theta1= atan2((Vc_y+Vc_w*rx),(Vc_x-Vc_w*ry))*180.0/pi
   theta2= atan2((Vc_y-Vc_w*rx),(Vc_x-Vc_w*ry))*180.0/pi
   theta3= atan2((Vc_y-Vc_w*rx),(Vc_x+Vc_w*ry))*180.0/pi
   theta4= atan2((Vc_y+Vc_w*rx),(Vc_x+Vc_w*ry))*180.0/pi
   
T=[[cos(theta1) 0 0 0];
   [sin(theta1) 0 0 0];
   [0 cos(theta2) 0 0];
   [0 cos(theta2) 0 0];
   [0 0 cos(theta3) 0];
   [0 0 cos(theta3) 0];
   [0 0 0 cos(theta4)];
   [0 0 0 cos(theta4)]];
%fkine = [Vc_x; Vc_y; Vc_w];
fkine =M*T*[v1; v2; v3; v4];
ikine=pinv(T)*K*[Vc_x; Vc_y; Vc_w]

m1 = ikine(1)*cos(theta1)
m2 = ikine(2)*cos(theta2)
m3 = ikine(3)*cos(theta3)
m4 = ikine(4)*cos(theta4)